\subsection{Варианты постановки задачи Коши}
Рассмотрим задачу Коши для обыкновенного дифференциального уравнения:
$$
\left\{
\begin{aligned}
&\dot x = f(x),\\
&x(0)=x_0\in x_0,
\end{aligned}
\right.
$$
\par Здесь, очевидно, состоянием в момент времени $t^*$ является точка $x(t^*)$.
Будем модифицировать классическую задачу:
\begin{equation}
\left\{
\begin{aligned}
&\dot x = f(x)\\
&x(0)=X_0.
\end{aligned}
\right.
\quad
\left\{
\begin{aligned}
&\dot x = f(x) + v_t\\
&x(0)=x_0.
\end{aligned}
\right.
\quad
\left\{
\begin{aligned}
&\dot x = f(x)\\
&x(0)=x_0\sim p(x_0).
\end{aligned}
\right.
\end{equation}
\begin{enumerate}
	\item $X_0$ --- множество. $X(t^*)$ --- множество достижимости.
	\item $v_t$ --- помеха, на которую наложены геометрические ограничения. Состояние $X(t^*)$
	так же является множеством.
	\item $p(x_0)$ --- распределение $x_0$. Такие системы называются стохастическими, в этом случае
	состояние -- это уже не точка (т.к. вероятность попадания
	в любую точку равна нулю), а плотность.
\end{enumerate}
\par Таким образом, состояние системы в определенный момент времени может выражаться в виде: 
\begin{itemize}
	\item точки
	\item множества
	\item плотности
	\item функции  (см. курс динамического программирования, задачи с запаздыванием).
\end{itemize}
\par Теперь от непрерывной задачи перейдем к дискретной, воспользовавшись представлением разностной
схемы:
\begin{gather*}
\dot{x} = Ax; \quad x = x(t), A = A(t)
\\
\Downarrow
\\
x_{k + 1} = (A\Delta t + \mOne)x_k = \tilde{A}_kx_k,
\end{gather*}
так как $A\Delta t$ мало, поэтому $\tilde A$ можно считать невырожденной.

Дискретный аналог задачи Коши:
$$
\left\{
\begin{aligned}
&x_{k+1}=f(x),\\
&x_0=x.
\end{aligned}
\right.
$$
\subsection{Основная модель}
Итак, рассмотрим соотношение
$$
x_{k+1}=\underbrace{f(x_k)}_{\E(x_{k + 1} \cp x_k)} + w(k),
$$
где $w(k)$ -- случайная помеха, $x_k,$ $w(k)$ независимы.

В данном дискретном случае состояние --- это совместная плотность по всем моментам времени $p(x_k,\dots, x_0),$ причем
\begin{multline*}
p(x_k,\dots,x_0) = p(x_k\left|x_{k-1},\dots,x_0\right.)p(x_{k-1}\left|x_{k-2},\dots,x_0\right.)\cdot\dots\cdotp(x_1\left|x_0\right.)\cdot p(x_0)=\\
=\left\{\text{марковский процесс}\right\}=p(x_k|x_{k-1})p(x_{k-1}|x_{k-2})\dots p(x_1|x_0)p(x_0).
\end{multline*}

\begin{df}
Марковский процесс (мартингал) --- процесс, который характеризуется отсутствием последствия.
\end{df}

Получаем \underline{реккурентную схему}.

Заметим, что в дискретном случае (речь идет о распределении $x_k$) плотность -- это вероятность, таким образом, состояние -- это обобщение плотности.

Схема $x_{k+1}=f(x_k)+w(k)$ говорит о знании последующего состояния через предыдущее состояние и текущие помехи.

Рассмотрим гауссовский случай, когда $w(k)$ и $x_k$ имеют гауссовское распределение.
В этом случае условное математическо ожидание $\E(x_{k+1}|x_k)$ линейно по $x_k.$
$$
x_{k+1}=A_kx_k+w_k
$$
--- линейное стохастическое дифференциальное уравнение.
В линейном случае $x_0,w_0,\dots, w_k$ --- гауссовские, поэтому $x_k,$ поэтому для их полного описания достаточно знать их первые и вторые моменты.

Пусть имеется статистическая задача $y=Ub+n$ (т.е. $b$ на месте, не изменяется).
Вводя понятие состояния превращаем ее в динамическую задачу: $x_{k+1}=A_kx_k+w_k$ ($x_k\sim b_k$) --- закон изменения искомого параметра (состояния).
\begin{note}
Потом дискретная схема будет с двух сторон приближать непрерывную.
\end{note}


\subsection{Задача для фильтра Калмана}
\begin{problem}
Найти ЛООСК $x_N$ относительно $Y_{N-1},$ где $Y_{N-1}=(y_0,\dots,y_{N-1})^T$ -- все наблюдения, при условии, что $x_0,w_0,\dots,w_{N-1},v_0,\dots,v_{N-1}$ независимы в совокупности. $\E x_0=\bar x_0,$ $\D x_0=S,$ $\E w_k=\E v_k = 0,$  $\D w_k=M_k,$ $\D v_k=N_k,$ $k=0,\dots, N-1.$
\end{problem}
\subsubsection{Две различные схемы}
Фильтр Калмана оценивает состояние в следующих схемах:

\begin{itemize}
\item I схема (<<мерим-шагаем>>)

$y_k=C_kx_k+v_k,$ $k=0,\dots,N-1$ -- измерение (уточнение, наблюдение)

$x_{k+1}=A_kx_k+w_k,$ $k=0,\dots, N-1$ -- шаг.

\item II схема (<<шагаем-мерим>>)

$x_{k+1}=A_kx_k+w_k,$ $k=0,\dots, N-1$

$y_{k+1}=C_{k+1}x_{k+1}+v_{k+1},$ $k=0,\dots, N-1.$
\end{itemize}

Таким образом, получаем два фильтра Калмана (для каждой схемы по фильтру).

\subsubsection{Ручное решение задачи}
Будем рассматривать первую схему. Как бы мы решали задачу оценивания $x_k,$ если бы не знали фильтра Калмана?

\begin{solution}
Перепишем первую схему в виде $x_k=L_{k-1}\tau_{k-1}$ -- уравнение динамики, $Y_k=\Phi_{k-1}r_{k-1}+v_k$ -- уравнение измерения, где $r_k=(x_0,w_0,\dots,w_k)^T,$ $L_k=(A_k\cdot\dots\cdot A_0, A_k\cdot\dots\cdot A_1, \dots, A_k, \mOne),$ $V_k=(v_0,\dots, v_k)^T,$ $Y_k=(y_0,\dots,y_k)^T,$
$$
\Phi_k=
\begin{pmatrix}
C_0 & 0 & \dots & 0 & 0 \\
C_1A_0 & C_1 & \dots & 0 & 0\\
\hdotsfor{5}\\
C_{k+1}A_k\cdot\dots\cdot A_0 & C_{k+1}A_k\cdot\dots\cdot A_1 & \dots & C_{k+1}A_k & C_{k+1}
\end{pmatrix},
$$
так как $y_k=C_kA_{K-1}A_{k-2}\cdot\dots\cdot A_0x_0+C_kA_{k-1}A_{k-2}\dots A_1w_0+C_kA_{k-1}A_{k-2}\dots A_2w_1+\dots+C_kA_{k-1}w_{k-2}+C_kw_{k-1}+v_k.$
Заметим, что задача быстро увеличивает размерность, что очень плохо.

ЛООСК для $r_{k-1}:$ 
\begin{multline*}
\hat r_{k-1}(Y_k)=\{\E (r_{k-1}|Y_k)\text{в гауссовском случае}\}=\\
=\mu_{r_{k-1}}+R_{r_{k-1}Y_k}R_{Y_k}^{-1}\left(Y_k-\mu_{Y_k}\right)=
\mu_{r_{k-1}}+
R_{r_{k-1}}\Phi^T_{k-1}
\left(\Phi_{k-1}R_{r_{k-1}}\Phi^T_{k-1}+R_{v_k}\right)^{-1}\left(Y_k-\Phi_{r_{k-1}}\mu_{r_{k-1}}\right)
\end{multline*}

ЛООСК для $x_k:$ (через уравнение связи)
$$
\hat x_k(Y_k)=\{=\E(x_k|Y_k)\}=L_{k-1}\hat r_{k-1}(Y_k).
$$
\qed
\end{solution}

В рассмотренном методе мы сначала развернули задачу к большой размерности, затем свернули к малой. Идея Фильтра Калмана: не переходить к большой размерности, а использовать факт марковского процесса. 


\subsubsection{Формулировка фильтра Калмана}
Введем следующие обозначения для оценок состояния и ошибки этих оценок
\begin{align*}
	\hat x_{k+1|k} &=\hat x_{k+1}(Y_k), \;\; \hat x_{k|k}=\hat x_k(Y_k), \\
	R_{k+1|k} &=\E(x_{k+1}-\hat x_{k+1|k})(x_{k+1}-\hat x_{k+1|k})^T, \;\; R_{k|k}=\E(x_k-\hat x_{k|k})(x_k-\hat x_{k|k})^T.
\end{align*}

В таких обозначениях нашей задачей будет вывести формулы для двухтактного перехода:
\begin{enumerate}
\item Шаг динамики: $\hat x_{k|k}\to \hat x_{k+1|k}$
\item Шаг измерения: $\hat x_{k+1|k}\to \hat x_{k+1|k}$
\end{enumerate}
Например, I схема (<<мерим-шагаем>>) примет вид
$$
\hat x_{0|1}\xrightarrow[\E(x_0|y_0)]{y_0}\hat x_{1|0}\xrightarrow[\E(x_1|y_0)]{шаг}\hat x_{1|0}\xrightarrow{y_1}\hat x_{1|1}\xrightarrow{шаг}\hat x_{2|1}\xrightarrow\dots
$$

Фильтр Калмана называется фильтром, т.к. он отфильтровываем информацию, на каждом шаге оставляя только новую.

\subsection{Построение фильтра для I схемы}
Продемонстрируем вывод формул фильтра Калмана для системы:
\begin{equation}
	\left\{
\begin{aligned}
&y_{k}=C_kx_k + v_k, \; k = \overline{0, N - 1}\\
&x_{k + 1}=A_kx_k + w_k, \; k = \overline{0, N - 1}.
\end{aligned}
\right.
\end{equation}
\subsubsection{Сделаем шаг динамики}
Рассмотрим сначала шаг динамики: $\hat x_{k|k}\to \hat x_{k+1|k}$. Из уравнения динамики получим

\begin{align*}
\hat x_{k+1|k} &= \E(x_{k+1}|Y_k) = A_k\E(x_k|Y_k) + \E w_k, \;\; \E(x_k|Y_k) = \hat x_{k|k} \\
R_{k+1|k} &= \E (x_{k+1}-\hat x_{k+1|k})(x_{k+1}-\hat x_{k+1|k})^T = \\
		  &= \E(A_kx_k+w_k-A_k\hat x_{k|k}-\E w_k)(A_kx_k+w_k-A_k\hat x_{k|k}-\E w_k)^T = \\
		  &= A_kR_{k|k}A^T_k+\D w_k,
\end{align*}
$x_k,w_k$ независимы.

\subsubsection{Сделаем шаг измерения}
Рассмотрим теперь шаг измерения: $\hat x_{k|k-1}\to \hat x_{k|k}$.
Наша задача --- учесть новое измерение $y_k$ и найти $\E (x_k|Y_{k-1},y_k) = \hat x_{k|k}$

Вспомним о теореме из раздела \ref{gauss_multidimentional}, утверждающей, что если
\begin{itemize}
\item $x_k,$ $Y_k,$ $y_k$ -- случайные векторы, имеющие совместное гауссовское распределение,
\item $Y_{k-1},$ $y_k$ независимы,
\end{itemize}
то $\E (x_k|Y_{k-1},y_k) = \E(x_k|Y_{k-1})+\E(x_k,y_k)-\E x_k.$
В данном случае $Y_{k-1}$ и $y_k$ не обязательно независимы, поэтому перед применением этой теоремы необходимо провести ортогонализацию (см в конце того же раздела):
$$
\hat x_{k|k} = \E(x_k|Y_{k-1}) + \E(x_k|y_k - \E(y_k|Y_{k-1})) - \E x_k,
$$
в таком представлении $y_k-\E(y_k|Y_{k-1})$ -- новая информация, ортогональна $Y_{k-1}.$

Итак, из уравнения измерения получаем получаем:
\begin{align*}
\hat x_{k|k} &= \hat x_{k|k-1}+\hat x_k(y_k-\hat y_k(Y_{k-1}))-\E x_k, \; \text{где}\\
\hat y(Y_{k-1})&=\E(y_k|Y_{k-1})=\E(C_kx_k+v_k|Y_{k-1})=C_k\E(x_k|Y_{k-1})+\E(v_k|Y_{k-1})=C_k\hat x_{k|k-1}+\E v_k \\
\D(y_k|Y_{k-1}) &= \E(y_k-\E(y_k|Y_{k-1}))(y_k-\E(y_k|Y_{k-1}))^T = \\
			    &=\E(C_kx_k+v_k-C_k\hat x_{k|k-1}-\E v_k)(C_kx_k+v_k-C_k\hat x_{k|k-1}-\E v_k)^T = \\
			    &=C_k\E(x_k-\hat x_{k|k-1})(x_k-\hat x_{k|k-1})^TC^T+\D v_k+C_k\E(x_k-\hat x_{k|k-1})(v_k-\E v_k)^T+\\
				&+\E(v_k-\E v_k)(x_k-\hat x_{k|k-1})^TC^T_k = \{v_k,x_k \text{независимы}\} = C_kR_{k|k-1}C^T_k+\D v_k
\end{align*}

Обозначим $e_k = y_k - \hat{y}_k(Y_{k - 1})$ --- вклад нового измерения. Для получения 
$\hat{x}_{k \cp k}$ осталось вычислить $\hat{x}_k(e_k)$:
\begin{equation*}
	\hat{x}_k(e_k) = \E(x_k \cp e_k) = \E x_k + R_{x_ke_k}R_{e_k}^{-1}(e_k - \E e_k)
\end{equation*}
\par Теперь выведем формулы для ошибки вклада нового измерения $R_{e_k}$ и ковариационной матрицы
ошибки и состояния $R_{x_ke_k}$:
\begin{align*}
	R_{e_k} &= \D e_k =
	\big\{ \E e_k = \E(C_kx_k + v_k) - C_k\E \hat{x}_{k \cp k - 1} - \E v_k = 0,
	\big.
	\\
	&\big.\text{ т.к. } \E\hat{x}_{k \cp k - 1} \E\hat{x}_{k \cp k - 1} = \E x_k
	\text{ --- несмещенная ЛООСК}
	\big\} =
	\\
&= \E e_ke_k^\T = \E\left(y_k - \hat{y}_k(Y_{k - 1})\right)\left(y_k - \hat{y}_k(Y_{k - 1})\right)
= \D(y_k \cp Y_{k - 1}) = C_k R_{k \cp k - 1}C_k^\T + \D v_k
\end{align*}

\begin{align*}
	R_{x_ke_k} 
	&= \E(x_k - \E x_k)(e_k - \E e_k)^\T = \E(x_k - \E x_k)e_k^\T = \E_ke_k^\T =
	\E x_k\left(y_k - \hat{y}_k(Y_{k - 1})\right)^\T 
	\\ &= 
	\E (x_k - \hat{x}_{k \cp k - 1})\left( y_k - \hat{y}_k(Y_{k - 1})\right)^\T + 
	\E \underbrace{\hat{x}_{k \cp k - 1}}_{\E(x_k \cp Y_{k - 1})}
	\underbrace{( y_k - \hat{y}_k(Y_{k - 1}))}_{\perp Y_{k - 1}} = \{ \E e_k = 0\} = 
	\\ &=
	\E(x_k - \hat{x}_{k\cp k - 1})( y_k - \hat{y}_k(Y_{k - 1}))^\T = 
	\E(x_k - \hat{x}_{k\cp k - 1})(C_kx_k + v_k - C_k\hat{x}_{k\cp k - 1} - \E v_k)^\T = 
	\\ &=
	R_{k \cp k - 1}C_k^\T + \E(x_k - \hat{x}_{k \cp k - 1})(v_k - \E v_k) =
	\{ x_k, v_k \text{ --- независимы }\} = R_{k \cp k - 1} C_k^\T 
\end{align*}
\par И в результате:
\begin{align*}
	\hat{x}_k(e_k) &= \E x_k + R_{k \cp k - 1}C_k^\T(C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1}
	(y_k - C_k\hat{x}_{k\cp k - 1} - \E v_k)
	\\
	\hat{x}_{k \cp k} &= \hat{x}_{k \cp k - 1} + \hat{x}_{k}(e_k) - \E x_k = 
	\\
	&= \hat{x}_{k \cp k - 1} + R_{k \cp k - 1}C_k^\T(y_k - C_k \hat{x}_{k \cp k - 1} - \E v_k)
\end{align*}
\begin{align*}
	R_{k \cp k} &= \E(x_k - \hat{x}_{k \cp k - 1})(x_k - \hat{x}_{k \cp k - 1})^\T = 
	\{ \text{подставляем $\hat{x}_{k \cp k}$} \} =
	\\ &= \E \left( x_k - \hat{x}_{k \cp k - 1} - R_{k \cp k - 1}C_k^\T
	(C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1} (C_k(x_k - \hat{x}_{k \cp k - 1}) + v_k - \E v_k)
	\right) (.)^\T =
	\\ &= 
	R_{k \cp k - 1} - R_{k \cp k - 1}C_k^\T (C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1}
	(C_kR_{k \cp k - 1} + \E(v_k - \E v_k)(x_k - \hat{x}_{k \cp k - 1})) - 
	\\ &-
	\left(
		\E (x_k - \hat{x}_{k \cp k - 1})(v_k - \E v_k)^\T + R_{k \cp k - 1}C_k^\T
	\right(
	(C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1}C_kR_{k \cp k - 1} +
	\\ &+
	R_{k \cp k - 1}C_k^\T(C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1}
	\E\left[ (C_k(x_k - \hat{x}_{k \cp k - 1}) + v_k - \E v_k)(.)^\T \right] \cdot
	\\ &\cdot (C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1} C_k R_{k \cp k - 1}
	= \{x_k, v_k \text{ --- независимы }\} = 
	\\ &=
	R_{k \cp k - 1} - 2R_{k \cp k - 1}C_k^\T(C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1}
	C_kR_{k \cp k - 1} + 
	\\ &+ 
	R_{k \cp k - 1}C_k^\T (C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1}
	(C_kR_{k \cp k - 1}C_k^\T + \D v_k)
	(C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1} C_k R_{k\cp k - 1} =
	\\ &=
	R_{k \cp k - 1} - R_{k \cp k - 1}C_k^\T(C_kR_{k \cp k - 1}C_k^\T + \D v_k)^{-1}
	 C_k R_{k \cp k - 1}
\end{align*}

\subsection{Окончательный вид фильтра Калмана}
\subsubsection{Фильтр Калмана для I схемы}
\begin{align*}
y_k &=C_kx_k+v_k, k=\overline{0,N-1}\\
x_{k+1} &=A_kx_k+w_k, k=\overline{0,N-1}\\
\text{где}\;\; &\E x_0=\bar x_0, \D x_0=s, \E w_k=0, \E v_k=0, \D w_k=M_k, \D v_k=N_k>0\\
\end{align*}
\begin{equation*}
\left\{
\begin{aligned}
\hat x_{k|k}&=\hat x_{k|k-1}+R_{k|k-1}C^T_k(C_kR_{k|k-1}C^T_k+N_k)^{-1}(y_k-C_k\hat x_{k|k-1}-\E v_k),\\
\hat x_{k+1|k}&=A_k\hat x_{k|k}+\E w_k,\\
R_{k|k}&=R_{k|k-1}-R_{k|k-1}C^T_k(C_kR_{k|k-1}C^T_k+N_k)^{-1}C_kR_{k|k-1},\\
R_{k+1|k}&=A_kR_{k|k}A^T_k+M_k,\\
\hat x_{0|-1}&=\bar x_0, \;\;\; R_{0|-1}=S,
\end{aligned}
\right.
\end{equation*}
при этом предполагается, что $x_0,w_0,\dots,w_{N-1},v_0\dots,v_{N-1}$  независимы в совокупности.

С помощью трех лемм можно получить различные представления, например, если к $R_{k|k}=(R^{-1}_{k|k-1}+C^T_kN^{-1}_kC_k)^{-1}$ применить лемму 1, то получится, $R_{k+1|k}=A_k(R^{-1}_{k|k-1}+C^T_kN^{-1}_kC_k)^{-1}A^T_k+M_k.$

\subsubsection{Фильтр Калмана для II схемы}
\begin{align*}
	x_{k+1}&=A_kx_k+w_k, \;\; k=\overline{0,N-1}\\
	y_{k+1}&=C_{k+1}x_{k+1}+v_{k+1}, \;\; k=\overline{0,N-1}\\
	\text{где} \;\; & \E x_0=\bar x_0, \D x_0=s, \E w_k=0, \E v_k=0, \D w_k=M_k, \D v_k=N_k>0\\
\end{align*}
\begin{equation*}
\left\{
\begin{aligned}
\hat x_{k+1|k}&=A_k\hat x_{k|k}+\E w_k,\\
\hat x_{k+1|k+1}&=\hat x_{k+1|k}+R_{k+1|k}C^T_{k+1}(C_{k+1}R_{k+1|k}C^T_{k+1}+N_{k+1})^{-1}(y_{k+1}-C_{k+1}\hat x_{k+1|k}-\E v_{k+1}),\\
R_{k+1|k}&=A_kR_{k|k}A^T_k+M_k,\\
R_{k+1|k+1}&=R_{k+1|k}-R_{k+1|k}C^T_{k+1}(C_{k+1}R_{k+1|k}C^T_{k+1}+N_{k+1})^{-1}C_{k+1}R_{k+1|k},\\
\hat x_{0|0}&=\bar x_0, \;\;\; R_{0|0}=S,
\end{aligned}
\right.
\end{equation*}
при этом предполагается, что $x_0,w_0,\dots,w_{N-1},v_0\dots,v_{N-1}$ независимы в совокупности.

Другое представление может иметь вид
\begin{multline*}
R_{k+1|k+1}=(R_{k+1|k}+C^T_{k+1}N^T_{k+1}C_{k+1})^{-1}\\
\hat x_{k+1|k+1}=\hat x_{k+1|k}+(R^{-1}_{k+1|k}+C^T_{k+1}N^{-1}_{k+1}C_{k+1})^{-1}C^T_{k+1}N^{-1}_{k+1}(y_{k+1}-C_{k+1}\hat x_{k+1|k}-\E v_{k+1})
\end{multline*}
